package legoLab02;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;


public class WallFollower2 {
	int desiredDistance = 35;
	int avgPower = 50;
	double gain = 0.5;
	int minPower = 40;
	int powerR, powerL;
	String l = "L";
	String r = " R";
	String eRror = "error";

	UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);


	public static void main(String[] args) throws InterruptedException {
		new WallFollower2().run();
	}

	public void run() throws InterruptedException
	{
		while(! Button.ESCAPE.isPressed())
		{
			int read = (int) us.getRange();
			int error = desiredDistance - read;

			powerL = avgPower + (int)(error * gain);
			powerR = avgPower - (int)(error * gain);

			LCD.clear();
			LCD.drawString(l + powerL + r + powerR, 0, 0);
			LCD.drawString(eRror + error, 0, 1);

			Car.forward(powerL, powerR);

			Thread.sleep(10);
		}
	}

}
